#! /usr/bin/env python
'''
    

'''
from threading import Thread
from time import sleep
from command_handler.msg import manual_cmd
import rospy

#自定义类
class ManCase(Thread):

 def __init__(self,queue):
   Thread.__init__(self)
   #获取话题名称
   topicName = rospy.get_param("ManualTopic","manual_cmd")
   #设置发布者publisher
   publisher = rospy.Publisher(topicName,manual_cmd,queue_size=10)
   self.publisher = publisher
   self.queue = queue
   self.cmd = None
   self.exit = False
   print("====================")
   print(self.publisher)

    

 def execute(self):
      print("Manual mode start!")
      while not self.exit:
         #时刻检查更新
         self.update()
         sleep(0.1)
      self.epilogue()

 def run(self):
    self.execute()

 def update(self):
     while(not self.queue.empty()):
         self.cmd = self.queue.get()
         #如果是退出命令，那么就停止这个进程
         if(self.cmd == "quit"):
            self.exit = True
         else:
         #如果不是退出命令，那么就继续进程
            print(self.cmd)
            rospy.loginfo("执行更新命令")
            #封装命令
            message = self.package(self.cmd)
            #发布命令
            self.publisher.publish(message)

 def package(self,cmd):
    #封装命令，将原装命令转换成对应的格式
    temp = manual_cmd()
    temp.carVelLin = cmd["carVel"][0]
    temp.carVelRot = cmd["carVel"][1] 
    temp.armPosPit = cmd["armPos"][0] 
    temp.armPosYaw = cmd["armPos"][1]
    temp.shoot = cmd["shoot"]
    return temp

 def epilogue(self):
   #  super().terminate()
   epilo = manual_cmd()
   epilo.carVelLin = 0
   epilo.carVelRot = 0
   epilo.armPosPit = 0
   epilo.armPosYaw = 0
   epilo.shoot = False
   #发布最终命令，停止小车运动
   self.publisher.publish(epilo) 
   print("/////////////////////////")
   rospy.loginfo("Manual Process ending")
   print("/////////////////////////")